﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows;

namespace WpfApplication1.Models
{
    public class Navigation
    {
        public static double RADIANTODEGREE = 57.2957795;
        public static double DEGREETORADIAN = 0.0174532925;

        public Vector GroundSpeed { get; set; }
        public Vector L { get; set; }
        public double angleEta { get; set; }
        public double accelerationSideways { get; set; }
        public Vector crosstrackError { get; set; }
        public double radius { get; set; }

        public double NAVLdamping { get; set; }
        public double NAVLperiod { get; set; }

        public Boolean intercept { get; set; }

        public Vector trajectoryStartPoint { get; set; }
        public Vector trajectoryEndPoint { get; set; }
        public Vector trajectory;
        public Vector interceptionTrajectory { get; set; }

        public Vector vehicle { get; set; }

        public Navigation(double NAVLdamping, double NAVLperiod)
        {
            this.NAVLdamping = NAVLdamping;
            this.NAVLperiod = NAVLperiod;

            //prevents infinit while loop in calcL crosstrackError
            this.trajectoryStartPoint = new Vector(0, 0);
            this.trajectoryEndPoint = new Vector(1, 1);
        }

        public void calcAccelerationSideways()
        {
            calcCrosstrackError();
            calcL();
            if (intercept)
            {
                Console.WriteLine("PMR-Info: Navigation interception not implemented");
            }
            else
            {
                calcAngleEta();
                accelerationSideways = 2 * (GroundSpeed.Length * GroundSpeed.Length) / L.Length * sinusDegree(angleEta);//radiant?
                Console.WriteLine(accelerationSideways);
            }
        }

        public void calcCrosstrackError()
        {
            crosstrackError = (Vector.Multiply(Trajectory, vehicle)/(Trajectory.Length * Trajectory.Length)) * Trajectory - vehicle;
        }

        public void calcAngleEta()
        {
            angleEta = Vector.AngleBetween(L, GroundSpeed);
        }

        public void calcL()
        {
            double length = lengthOfL();
            //noch nicht optmiert, läuft immer wieder von horizontal bis schnitpunkt und nicht letzer wert von L
            int i = 45;
            double estimatedL = crosstrackError.Length / cosinusDegree(i);
            if (estimatedL > length)
            {
                intercept = true;
                interceptionTrajectory = new Vector(cosinusDegree(i) * crosstrackError.X - sinusDegree(i) * crosstrackError.Y, sinusDegree(i) * crosstrackError.X + cosinusDegree(i) * crosstrackError.Y);
                //interceptionTrajectory.X = Math.Cos(i) * crosstrackError.X - Math.Sin(i) * crosstrackError.Y;
                //interceptionTrajectory.Y = Math.Sin(i) * crosstrackError.X + Math.Cos(i) * crosstrackError.Y;

                //test purpose online
                intercept = false;
                if (!double.IsNaN((Math.Sqrt(length * length - crosstrackError.Length * crosstrackError.Length))))
                {
                    L = crosstrackError + (Math.Sqrt(length * length - crosstrackError.Length * crosstrackError.Length)) * Vector.Divide(Trajectory, Trajectory.Length);
                }
                else
                {
                    L = crosstrackError;
                }

            }else {
                intercept = false;
                //L = crosstrackError + (L - crossTrackError)
                //(L - crosstrackError) = p
                // p = |p| * ( trajectory / |trajectory| )
                // |p| = sqrt( |L|^2 - |crosstrackError|^2 )
                L = crosstrackError + (Math.Sqrt(length * length - crosstrackError.Length * crosstrackError.Length)) * Vector.Divide(Trajectory, Trajectory.Length);
            }
        }

        public static Vector rotateVector(Vector a, double angle)
        {
            return new Vector(cosinusDegree(angle) * a.X - sinusDegree(angle) * a.Y, sinusDegree(angle) * a.X + cosinusDegree(angle) * a.Y);
        }

        public double lengthOfL()
        {
            return 1 / Math.PI * NAVLdamping * NAVLperiod * GroundSpeed.Length;
        }

        public Vector Trajectory
        {
            get{
                if (trajectory == null || trajectory.X==0&&trajectory.Y==0)
                {
                    trajectory = trajectoryEndPoint - trajectoryStartPoint;
                }
                return trajectory;
            }
        }

        public static double cosinusDegree(double input)
        {
            return Math.Cos(input * DEGREETORADIAN);
        }

        public static double sinusDegree(double input)
        {
            return Math.Sin(input * DEGREETORADIAN);
        }
    }
}
